"""
Lab 4 - Joystick ADC Interface
ESSE 2220
Reads joystick position using ADS7830 ADC module via I2C
"""

import RPi.GPIO as GPIO
import numpy as np
import smbus
import time

# ============================================================================
# GPIO PIN CONFIGURATION
# ============================================================================
Z_PIN = 18          # GPIO pin for joystick Z-axis (button)
SDA_PIN = 2         # GPIO pin for I2C SDA
SCL_PIN = 3         # GPIO pin for I2C SCL

# ============================================================================
# I2C CONFIGURATION
# ============================================================================
ADC_ADDRESS = 0x4b  # I2C address for ADS7830 ADC module
ADC_CMD = 0x84      # Command byte for ADS7830

# ============================================================================
# ADC DEVICE CLASSES
# ============================================================================
class ADCDevice(object):
    """Base class for ADC devices with I2C communication"""
    
    def __init__(self):
        """Initialize I2C bus communication"""
        self.bus = smbus.SMBus(1)  # 1 indicates /dev/i2c-1


class ADS7830(ADCDevice):
    """ADS7830 8-channel 8-bit ADC module"""
    
    def __init__(self, address=ADC_ADDRESS):
        """
        Initialize ADS7830 ADC
        
        Args:
            address: I2C address (default: 0x4b)
        """
        super(ADS7830, self).__init__()
        self.cmd = ADC_CMD
        self.address = address
        
    def analogRead(self, channel):
        """
        Read analog value from specified ADC channel
        
        Args:
            channel: ADC input channel (0-7)
            
        Returns:
            int: Analog reading (0-255)
        """
        if not 0 <= channel <= 7:
            raise ValueError("Channel must be between 0 and 7")
            
        # Calculate channel command byte
        channel_cmd = self.cmd | (((channel << 2 | channel >> 1) & 0x07) << 4)
        value = self.bus.read_byte_data(self.address, channel_cmd)
        return value

# ============================================================================
# SETUP AND UTILITY FUNCTIONS
# ============================================================================
def setup():
    """
    Initialize GPIO pins and ADC device
    
    Returns:
        ADS7830: Initialized ADC object
    """
    GPIO.setmode(GPIO.BCM)
    GPIO.setup(Z_PIN, GPIO.IN)
    adc = ADS7830()
    return adc


def detectI2C(adc, addr):
    """
    Detect if I2C device exists at specified address
    
    Args:
        adc: ADC device object
        addr: I2C address to check
        
    Returns:
        bool: True if device found, False otherwise
    """
    try:
        adc.bus.write_byte(addr, 0)
        print(f"Found device at address 0x{addr:02x}")
        return True
    except Exception as e:
        print(f"No device found at address 0x{addr:02x}")
        return False


def getUserInput():
    """
    Prompt user for initial and final coordinates
    
    Returns:
        numpy.ndarray: 2x2 array of [initial, final] coordinates
    """
    print("\n=== Coordinate Input ===")
    
    # Get initial coordinates
    init_x = float(input("Enter initial x coordinate: "))
    init_y = float(input("Enter initial y coordinate: "))
    initialCoords = [init_x, init_y]
    
    # Get final coordinates
    final_x = float(input("Enter final x coordinate: "))
    final_y = float(input("Enter final y coordinate: "))
    finalCoords = [final_x, final_y]
    
    return np.array([initialCoords, finalCoords])


def readRawJoystickPosition(adc):
    """
    Read current joystick X and Y position from ADC
    
    Args:
        adc: ADS7830 ADC object
        
    Returns:
        tuple: (x_value, y_value) as integers (0-255)
    """
    x_value = adc.analogRead(0)  # Channel 0 for X-axis
    y_value = adc.analogRead(1)  # Channel 1 for Y-axis
    return x_value, y_value

def fixRawToCalibrated(x_raw, y_raw, center, scale, tol, deadZone=0.1):
    """
    Convert raw ADC values to calibrated coordinate values with dead zone
    
    Transforms raw joystick readings (0-255) to normalized coordinates centered
    at zero. Applies a dead zone to filter out small unintentional movements.
    
    Args:
        x_raw: Raw X-axis ADC value (0-255)
        y_raw: Raw Y-axis ADC value (0-255)
        center: Center position of joystick (typically 128 for 8-bit ADC)
        scale: Scaling factor for output range
        tol: Tolerance value (unused in current implementation)
        deadZone: Dead zone threshold for ignoring small movements (default: 0.1)
        
    Returns:
        numpy.ndarray: Calibrated [x, y] coordinates
    """
    # Apply dead zone to filter out small unintentional movements
    # If movement is within dead zone, treat as centered (no movement)
    if abs((x_raw - center) / 127 * scale) < deadZone:
        x_raw = center
    if abs((y_raw - center) / 127 * scale) < deadZone:
        y_raw = center
    
    # Normalize coordinates: center at 0, scale by range and factor
    # Divide by 127 (half of 255) to get range of approximately -1 to 1
    nx = ((x_raw - center) / 127) * scale
    ny = ((y_raw - center) / 127) * scale
    
    return np.array([nx, ny])


def updatePosition(currentPos, delta, isBoostEnabled, boostFactor):
    """
    Update current position based on joystick movement with optional boost
    
    Calculates new position by applying delta movement to current position.
    If boost is enabled (Z-button pressed), movement is amplified by boost factor.
    
    Args:
        currentPos: Current position as numpy array [x, y]
        delta: Movement delta from joystick as numpy array [dx, dy]
        isBoostEnabled: Boolean indicating if boost mode is active (Z-button pressed)
        boostFactor: Multiplier for movement when boost is enabled
        
    Returns:
        numpy.ndarray: New position [x, y] after applying movement
    """
    # Apply boost multiplier if Z-button is pressed
    if isBoostEnabled:
        delta *= boostFactor
    
    # Calculate new position by adding delta to current position
    newPos = currentPos + delta
    return newPos

def comparePositions(pos1, pos2, tol):
    """
    Check if two positions are within tolerance of each other
    
    Determines if the distance between two positions is within the specified
    tolerance for both X and Y coordinates. Used to check if target has been reached.
    
    Args:
        pos1: First position as numpy array [x, y]
        pos2: Second position as numpy array [x, y]
        tol: Maximum allowed difference for each coordinate
        
    Returns:
        bool: True if both coordinates are within tolerance, False otherwise
    """
    # Check if absolute difference in both X and Y is within tolerance
    return np.all(np.abs(pos1 - pos2) <= tol)

# ============================================================================
# MAIN PROGRAM
# ============================================================================
def main():
    """Main program execution"""
    # Initialize hardware
    adc = setup()
    
    # Get user input for coordinates
    coords = getUserInput()
    initialPos = coords[0]
    finalPos = coords[1]
    
    # Display coordinate information
    print("\n=== Coordinate Summary ===")
    print(f"Initial Coordinates: {initialPos}")
    print(f"Final Coordinates: {finalPos}")
    
    # Read and display initial joystick position
    x_pos, y_pos = readRawJoystickPosition(adc)
    print(f"\n=== Initial Joystick Position ===")
    print(f"X: {x_pos}, Y: {y_pos}")
    print(f"Raw: ({x_pos}, {y_pos})")
    
    # Calibration constants
    CENTER = 128         # Center value for 8-bit ADC (midpoint of 0-255)
    SCALE = 1            # Scaling factor for coordinate normalization
    TOL = 0.5            # Tolerance for position comparison (units)
    boostFactor = 1.5    # Speed multiplier when Z-button is pressed
    
    print(f"Calibrated: ({fixRawToCalibrated(x_pos, y_pos, CENTER, SCALE, TOL)})")

    # Position tracking control loop
    reachedTarget = False
    
    # Check if already at target position
    if comparePositions(initialPos, finalPos, TOL):
        reachedTarget = True
        print("Initial position is the same as final position. No movement needed.")
    else:
        # Main control loop - continue until target is reached
        while not reachedTarget:
            # Read current joystick position
            x_pos, y_pos = readRawJoystickPosition(adc)
            calibratedPos = fixRawToCalibrated(x_pos, y_pos, CENTER, SCALE, TOL)
            print(f"\nCurrent Joystick Position: {calibratedPos}")
            
            # Check if Z-button (boost) is pressed (active LOW)
            isBoostEnabled = GPIO.input(Z_PIN) == GPIO.LOW
            
            # Update current position based on joystick input and boost status
            initialPos = updatePosition(initialPos, calibratedPos, isBoostEnabled, boostFactor)
            print(f"Updated Position: {initialPos}")
            
            if isBoostEnabled:
                print("Boost Enabled on this movement!")
            
            # Check if target position is reached within tolerance
            if np.all(np.abs(initialPos - finalPos) <= TOL):
                reachedTarget = True
                print("Reached target position!")
            
            time.sleep(0.5)  # Update interval for smooth control




if __name__ == "__main__":
    try:
        main()
    except KeyboardInterrupt:
        print("\n\nProgram terminated by user")
    except Exception as e:
        print(f"\nError: {e}")
    finally:
        GPIO.cleanup()
        print("GPIO cleanup complete")
ESSE 2220 – Lab 4 Report¶

Joystick Space Navigation¶

1. Names & Group Info¶

2. Setup:¶

2.1 Circuit:¶

2.2 Main Program:¶

3. Demo¶

4. Observations¶

4.1 Describe how joystick tilt (X/Y) changed your ship’s movement.¶

4.2 How did adjusting SCALE or TOL affect control and arrival?¶

4.3 Did the ship drift when you released the joystick? Why?¶

5. Analysis¶

5.1 What is the starting point and how is it set?¶

5.2 What is the target location and how do you check arrival?¶

5.3 Why do we use TOL (tolerance) instead of requiring exact coordinates?¶

5.4 What would happen if you didn't use scaling and just added the raw joystick values to the position?¶

6. Bonus¶
